/*
 * =====================================================================================
 *
 *       Filename:  main.c
 *
 *    Description:  
 *
 *        Version:  1.0
 *        Created:  2020年03月05日 11时33分50秒
 *       Revision:  none
 *       Compiler:  gcc
 *
 *         Author:  YOUR NAME (), 
 *   Organization:  
 *
 * =====================================================================================
 */
#include "util_net.h"
#include "util_math.h"
#include "util_time.h"
#include "util_debug.h"
#include "modbus_tcp.h"
#include <signal.h>
#include <pthread.h>

int send_err = 0;


int response_errcode(int fd, MBAP *mbap, uint8_t opcode, uint8_t errcode)
{
    uint8_t *buff = (uint8_t *)calloc(1, sizeof(MBAP) + sizeof(RSP_PDU_0x01_err));
    if(NULL == buff) {
        DEBUG_ERR(__func__, "calloc");
        return 1;
    }

    MBAP *rsp_mbap = (MBAP *)buff;
    RSP_PDU_0x01_err *rsp_pdu_err = (RSP_PDU_0x01_err *)(buff + sizeof(MBAP));

    memcpy(rsp_mbap, mbap, sizeof(MBAP));
    rsp_mbap->length = htons(1 + (sizeof(RSP_PDU_0x01_err)));

    rsp_pdu_err->opcode = opcode + 0x80;
    rsp_pdu_err->errcode = errcode;
    int ret = write(fd, buff, sizeof(MBAP) + sizeof(RSP_PDU_0x01_err));
    if(ret <= 0) {
        DEBUG_ERR(__func__, "write");
    }
    else {
        int i;
        debug_print("Tx:");
        for(i = 0; i < sizeof(MBAP) + sizeof(RSP_PDU_0x01_err); i++) {
            debug_print("%02x ", buff[i] & 0xff);
        }
        debug_print("\n\n");
    }
    free(buff);
    return ret;
}

int process_opcode_0x01(int fd, MBAP *mbap)
{
    REQ_PDU_0x01 *req = (REQ_PDU_0x01 *)(((uint8_t *)mbap) + sizeof(MBAP));
    int opcode = req->opcode;
    uint16_t address = ntohs(req->address);
    uint16_t count   = ntohs(req->count);
    debug_print("0x01: address=0x%04X, count=0x%04X, count=%d\n", address, count, count);
    
    if(count < 1 || count > 0x07D0) {
        return response_errcode(fd, mbap, opcode, 0x03);
    }

    if( ((address < 0x0000) || (address > 0xFFFF)) ||
        (0xFFFF - address < count)) {
        /*
         * 地址不在范围内或者address+count>0xffff
         */
        debug_print("Address or Count is not correct\n");
        return response_errcode(fd, mbap, opcode, 0x02);
    }
    
    /* 输出大小 */
    uint8_t bytes = (count/8) + (count%8?1:0);
    if(send_err) {
        debug_print("正确的bytes应为%d，为了发送错误报文，我们设置bytes=%d\n", bytes, bytes/2);
        bytes = bytes/2;
    }

    uint8_t *buff = (uint8_t *)calloc(1, sizeof(MBAP) + sizeof(RSP_PDU_0x01) + bytes);
    if(NULL == buff) {
        DEBUG_ERR(__func__, "calloc");
        return response_errcode(fd, mbap, opcode, 0x04);
    }
    
    MBAP *rsp_mbap = (MBAP *)buff;
    RSP_PDU_0x01 *rsp_pdu = (RSP_PDU_0x01 *)(buff + sizeof(MBAP));

    memcpy(rsp_mbap, mbap, sizeof(MBAP));
    rsp_mbap->length = htons(1 + (sizeof(RSP_PDU_0x01) + bytes));

    rsp_pdu->opcode = opcode;
    rsp_pdu->bytes = bytes;

    int ret = write(fd, buff, sizeof(MBAP) + sizeof(RSP_PDU_0x01) + bytes);
    if(ret <= 0) {
        DEBUG_ERR(__func__, "write");
    }
    else {
        int i;
        debug_print("Tx:");
        for(i = 0; i < sizeof(MBAP) + sizeof(RSP_PDU_0x01) + bytes; i++) {
            debug_print("%02x ", buff[i] & 0xff);
        }
        debug_print("\n\n");
    }
    free(rsp_mbap);
    return ret;
}

void thread_modbus_server(void *arg)
{
    int fd = *((int *)arg);
    free(arg);
    pthread_detach(pthread_self());

    while(1) {
        /* recv */
        char buff[2048] = {0};
        int ret = read(fd, buff, sizeof(buff));
        if(ret < 0) {
            DEBUG_ERR(__func__, "read");
            close(fd);
            break;
        }
        else if(ret == 0) {
            debug_print("Peer Say bye\n");
            close(fd);
            break;
        }

        int i;
        debug_print("Rx:");
        for(i = 0; i < ret; i++) {
            debug_print("%02x ", buff[i] & 0xff);
        }
        debug_print("\n");

        /* process */
        if(ret < 7) {
            debug_err("%s(): read ret=%d, less than 7, Wrong Request\n", __func__, ret);
            continue;
        }

        MBAP *mbap = (MBAP *)buff;
        uint8_t opcode = buff[sizeof(MBAP)];
        switch(opcode) {
        case 0x01:
        case 0x02:
            if(ret != (sizeof(MBAP) + sizeof(REQ_PDU_0x01))) {
                debug_err("opcode 0x01, Recv length should be 12 but %d\n", ret);
            }
            ret = process_opcode_0x01(fd, mbap);
            break;
        default:
            debug_print("Do not Support opcode 0x%02x\n", opcode);
            ret = response_errcode(fd, mbap, opcode, 0x01);
            break;
        }
        if(ret <= 0) {
            close(fd);
            break;
        }
    }
    
    pthread_exit(NULL);
}


int main(int argc, char **argv)
{
    if(argc == 2) {
        send_err = 1;
        debug_print("send error message on\n");
    }
    int fd_s = tcpsock_create("0.0.0.0", 502, 10);
    if(fd_s < 0) {
        debug_err("%s(): tcpsock_create() failed\n", __func__);
        return 0;
    }
    
    if(SIG_ERR == signal(SIGPIPE, SIG_IGN)) {
        return DEBUG_ERR(__func__, "signal");
    }
    
    while(1) {
        struct sockaddr_in addr;
        socklen_t addr_len = sizeof(addr);
        int fd = accept(fd_s, (struct sockaddr *)&addr, &addr_len);
        if(fd < 0) {
            DEBUG_ERR(__func__, "accept");
            if(errno != SIGINT) {
                break;
            }
            else {
                continue;
            }
        }

        /* 线程处理 */ 
        
        int *fd_p = (int *)calloc(1, sizeof(int));
        if(NULL == fd_p) {
            DEBUG_ERR(__func__, "calloc");
            close(fd);
            continue;
        }

        *fd_p = fd;

        pthread_t th;
        if(0 != pthread_create(&th, NULL, (void *)&thread_modbus_server, (void *)fd_p)) {
            DEBUG_ERR(__func__, "pthread_create");
            close(fd);
            free(fd_p);
            continue;
        }
    }
    debug_print("%s() exit", __func__);
    return 0;
}

